/*
 * Compensation.c
 *
 *  Created on: Oct 4, 2024
 *      Author: Grasping
 */
/*======================================Compensation function======================================*/
#include "My_INC/PHY/PHY_Compensation.h"

/**
 * @brief 根据给定的斜率和截距对原始数据进行补偿
 * @param raw_data 原始数据值
 * @param slope 补偿曲线的斜率
 * @param intercept 补偿曲线的截距
 * @return 补偿后的数据值
 */
float Compensate(uint16_t raw_data, float slope, float intercept) { return ((float)slope * raw_data) + intercept; }

/*==================================Variable step initialization===================================*/
/**
 * @brief 可变步长控制器的初始化函数
 * @param VariableStep 可变步长控制器的指针，用于存储控制器的状态信息
 * @param frequency 控制器的执行频率（KHz），例如 50 表示 50kHz
 * @param duration 控制器的执行时长（ms），例如 50 表示 50ms
 */

static inline void VariableStep_Reset_Inline(VariableStep_Typedef *VariableStep, float start_num);
void VariableStep_Init(VariableStep_Typedef *VariableStep, float frequency, float duration, float start_num)
{
    if (VariableStep == NULL)
        return;
    VariableStep->totalSteps = (uint32_t)(frequency * duration); // 计算总步数为频率 * 时长
    VariableStep_Reset_Inline(VariableStep, start_num);          // 重置可变步长控制器
}
void VariableStep_Reset(VariableStep_Typedef *VariableStep, float start_num)
{
    if (VariableStep == NULL)
        return;
    VariableStep_Reset_Inline(VariableStep, start_num); // 重置可变步长控制器
}

static inline void VariableStep_Reset_Inline(VariableStep_Typedef *VariableStep, float start_num)
{
    VariableStep->currentTemp = start_num;              // 初始化当前的参考值为 0
    VariableStep->previousReference = 0.0f;             // 初始化前一时刻的参考值为 0
    VariableStep->currentStep = 0;                      // 初始化当前的步数为 0
    VariableStep->incrementStep = 0.0f;                 // 初始化每一步的增量为 0
    VariableStep->state = VARIABLE_STEP_STATE_COMPLETE; // 初始化状态为计算步长
}
/*
 * @brief 可变步长控制器的更新函数
 * @param VariableStep 可变步长控制器的指针，用于存储控制器的状态信息
 * @param currentReference 当前的目标参考值，即期望值
 * @return 返回当前的参考值
 */
float VariableStep_Update(VariableStep_Typedef *VariableStep, float currentReference)
{
    switch (VariableStep->state)
    {
    case VARIABLE_STEP_STATE_CALE:
        VariableStep->incrementStep = (VariableStep->currentRef - VariableStep->currentTemp) /
                                      (float)VariableStep->totalSteps; // 重新计算每一步的增量
        VariableStep->previousReference = VariableStep->currentRef;    // 更新前一时刻的参考值为当前期望值
        VariableStep->currentStep = 0;                                 // 重置当前步数和完成标志
        VariableStep->state = VARIABLE_STEP_STATE_RUNNING;             // 设置状态为运行
        break;
    case VARIABLE_STEP_STATE_RUNNING:
        VariableStep->currentTemp += VariableStep->incrementStep; // 逐步增加当前参考值
        if (VariableStep->currentStep++ > VariableStep->totalSteps)
        {
            VariableStep->currentTemp = VariableStep->previousReference; // 确保最终参考值的准确性
            VariableStep->state = VARIABLE_STEP_STATE_COMPLETE;          // 设置状态为完成
        }
        break;
    case VARIABLE_STEP_STATE_COMPLETE:
        VariableStep->state = VARIABLE_STEP_STATE_COMPLETE; // 设置状态为计算步长
        VariableStep->currentRef = currentReference;
        if (VariableStep->currentRef < 0.0f)
            VariableStep->currentRef = 0.0f; // 防止参考值小于 0
        if (VariableStep->currentRef != VariableStep->previousReference)
            VariableStep->state = VARIABLE_STEP_STATE_CALE;
        break;
    default:
        VariableStep->state = VARIABLE_STEP_STATE_COMPLETE; // 设置状态为计算步长
        break;
    }
    return VariableStep->currentTemp;
}
